
import socket
import json
import copy
import time

import yaml
import os
import traceback
import sys
import numpy as np

import cv2
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Bool,String
from acupoint_msgs.msg import AcupointImagePixel

# import sys
# sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')


from cv_bridge import CvBridge, CvBridgeError



class CameraDisplay:
    def __init__(self):
        self.actions=self.loadYaml('./exp.yaml')
        if self.actions is None:
            print("Error, load yaml file failed")
            return
       
        # print(self.agv_postions)
         
        self.actions=self.actions['actions']
        self.deg2rad=3.1415926/180
        self.origin_image=None
        self.bridge=CvBridge()
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu_filter_image_pixel", AcupointImagePixel, self.acupointCallback) 
        self.img_sub = rospy.Subscriber("/k4a/color/color", Image,self.imgCallback,queue_size=1)
        self.jaka_arrive=rospy.Subscriber("/acupoint_name",String,self.jakaCallback)
        self.x_lists=[]
        self.y_lists=[]
        self.jaka_arrive=False
        self.acupoint_name=None
        self.display_acupoint=None
        while self.origin_image is None or self.acupoint_name is None:
            rospy.sleep(0.1)
        
        # self.client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        
        try:
            # self.client.connect(('10.161.229.76',56789))
            index=0
            massage_actions=[]
            while not rospy.is_shutdown():
                if self.display_acupoint=="end":
                    self.display_acupoint=None
                if self.display_acupoint is not None:
                    index=self.acupoint_name.index(self.display_acupoint)
                    x=self.acupoint_pixels[index].x
                    y=self.acupoint_pixels[index].y
                    y_plot = int(x)
                    x_plot = int(y)
                    cv2.circle(self.origin_image, (x_plot, y_plot), 10, (255, 255, 255), thickness=-1)
                cv2.namedWindow("camera_display", 0)
                cv2.imshow("camera_display", self.origin_image)
                cv2.waitKey(100)   
        except:
            print("connect failed")
            print(traceback.format_exc())
        finally:
            pass
            # self.client.close()
    
    def jakaCallback(self,msg):
        self.display_acupoint=msg.data
        # self.jaka_arrive=msg.data   
         
    def imgCallback(self,image_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
            self.origin_image = cv_image
            
        
        except CvBridgeError as e:
            print(e)
    
  
            
      
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_pixels=copy.deepcopy(msg).pixels
     
    

        
    def loadYaml(self,file_path):
        if not os.path.exists(file_path):
            print("Error, %s not exists"%file_path)
            return
        yaml_data=None
        try:
            with open(file_path) as f:
                yaml_data=yaml.safe_load(f.read())
        except:
            print("Some error occured,the exception message is:")
            print(traceback.format_exc())
        return yaml_data


if __name__ == "__main__":
    rospy.init_node('camera_display_node')
    cd=CameraDisplay()



    

    
